#!/usr/bin/env python


import roslib; roslib.load_manifest('intersector')
import rospy
import copy

import intersector.msg
from intersector.msg import Plane
from intersector.msg import Convex_Space
from std_msgs.msg import String



def inclusive_range(start,end, step=1):
    assert step != 0
    if step > 0:
        return range(start,end+1, step)
    else:
        return range(start,end-1, step)



def init():
    output_name = "intersector_input"

    publisher = rospy.Publisher(output_name, Convex_Space)
    rospy.init_node('intersector')

    print "Sleep for 2 seconds to allow the 3d_intersector time to initialize..."
    rospy.Rate(.5).sleep()

    print "Sleep done..."



    print "Sending recorded shape information..."


# the following was recorded from "rostopic echo /intersector_input" during a run of one of our Gazebo tests
# ---
# planes: [
#     a: 0.0
#     b: 0.0
#     c: 1.0
#     d: -1.0,
#     a: 0.0
#     b: 0.0
#     c: -1.0
#     d: 0.0]
# sensor: horizontal_slicer
# algorithm: horizontal_slicer
# aux_payload: slicer:horizontal
# ---
# planes: [
#     a: -0.0507812052965
#     b: 0.0
#     c: -0.0324326232076
#     d: -0.0972978696227,
#     a: 3.45435147153e-08
#     b: 0.048828125
#     c: -0.0260353088379
#     d: -0.0781059265137,
#     a: 0.0507812090218
#     b: 0.0
#     c: 0.0299530699849
#     d: 0.0898592099547,
#     a: -3.12536556635e-08
#     b: -0.048828125
#     c: 0.0235557556152
#     d: 0.0706672668457]
# sensor: /overhead_cam
# algorithm: ian3
# aux_payload: ian3:blue
# ---
# planes: [
#     a: -5.19797671572e-09
#     b: -0.025390625
#     c: 0.0039176940918
#     d: 0.0117530822754,
#     a: -0.0253906045109
#     b: 0.0
#     c: -0.0159187652171
#     d: -0.0477562956512,
#     a: 6.05333960735e-09
#     b: 0.025390625
#     c: -0.00456237792969
#     d: -0.0136871337891,
#     a: 0.0253906045109
#     b: 0.0
#     c: 0.0152740813792
#     d: 0.0458222441375]
# sensor: /overhead_cam
# algorithm: ian3
# aux_payload: ian3:white
# ---
# planes: [
#     a: 6.38232577899e-09
#     b: -0.025390625
#     c: -0.00481033325195
#     d: -0.0144309997559,
#     a: -0.0253906026483
#     b: 0.0
#     c: -0.0162163116038
#     d: -0.0486489348114,
#     a: -5.52696244327e-09
#     b: 0.025390625
#     c: 0.00416564941406
#     d: 0.0124969482422,
#     a: 0.0253906045109
#     b: 0.0
#     c: 0.0155716277659
#     d: 0.0467148832977]
# sensor: /overhead_cam
# algorithm: ian3
# aux_payload: ian3:green
# ---
# planes: [
#     a: 3.46700481657e-08
#     b: -0.048828125
#     c: -0.0261306762695
#     d: -0.0783920288086,
#     a: -0.0507812052965
#     b: 0.0
#     c: -0.0324326232076
#     d: -0.0972978696227,
#     a: -3.13801891139e-08
#     b: 0.048828125
#     c: 0.0236511230469
#     d: 0.0709533691406,
#     a: 0.0507812090218
#     b: 0.0
#     c: 0.0299530699849
#     d: 0.0898592099547]
# sensor: /overhead_cam
# algorithm: ian3
# aux_payload: ian3:red
# ---


    space = Convex_Space()
    space.sensor      = "horizontal_slicer"
    space.algorithm   = "horizontal_slicer"
    space.aux_payload = "slicer:horizontal"

    space.planes = []

    plane = Plane()
    plane.a =  0
    plane.b =  0
    plane.c =  1
    plane.d = -1
    space.planes.append(plane)

    plane = Plane()
    plane.a =  0
    plane.b =  0
    plane.c = -1
    plane.d =  0
    space.planes.append(plane)

#    print "PUBLISHING: %s" % space
    publisher.publish(space)

    # ----

    space = Convex_Space()
    space.sensor      = "/overhead_cam"
    space.algorithm   = "ian3"
    space.aux_payload = "ian3:blue"

    space.planes = []

    plane = Plane()
    plane.a =  -0.0507812052965
    plane.b =   0.0
    plane.c =  -0.0324326232076
    plane.d =  -0.0972978696227
    space.planes.append(plane)

    plane = Plane()
    plane.a =  3.45435147153e-08
    plane.b =  0.048828125
    plane.c = -0.0260353088379
    plane.d = -0.0781059265137
    space.planes.append(plane)

    plane = Plane()
    plane.a = 0.0507812090218
    plane.b = 0.0
    plane.c = 0.0299530699849
    plane.d = 0.0898592099547
    space.planes.append(plane)

    plane = Plane()
    plane.a = -3.12536556635e-08
    plane.b = -0.048828125
    plane.c =  0.0235557556152
    plane.d =  0.0706672668457
    space.planes.append(plane)

#    print "PUBLISHING: %s" % space
    publisher.publish(space)

    # ----

    space = Convex_Space()
    space.sensor      = "/overhead_cam"
    space.algorithm   = "ian3"
    space.aux_payload = "ian3:white"

    space.planes = []

    plane = Plane()
    plane.a = -5.19797671572e-09
    plane.b = -0.025390625
    plane.c = 0.0039176940918
    plane.d = 0.0117530822754
    space.planes.append(plane)

    plane = Plane()
    plane.a = -0.0253906045109
    plane.b = 0.0
    plane.c = -0.0159187652171
    plane.d = -0.0477562956512
    space.planes.append(plane)

    plane = Plane()
    plane.a = 6.05333960735e-09
    plane.b = 0.025390625
    plane.c = -0.00456237792969
    plane.d = -0.0136871337891
    space.planes.append(plane)

    plane = Plane()
    plane.a = 0.0253906045109
    plane.b = 0.0
    plane.c = 0.0152740813792
    plane.d = 0.0458222441375
    space.planes.append(plane)

#    print "PUBLISHING: %s" % space
    publisher.publish(space)

    # ----

    space = Convex_Space()
    space.sensor      = "/overhead_cam"
    space.algorithm   = "ian3"
    space.aux_payload = "ian3:green"

    space.planes = []

    plane = Plane()
    plane.a = 6.38232577899e-09
    plane.b = -0.025390625
    plane.c = -0.00481033325195
    plane.d = -0.0144309997559
    space.planes.append(plane)

    plane = Plane()
    plane.a = -0.0253906026483
    plane.b = 0.0
    plane.c = -0.0162163116038
    plane.d = -0.0486489348114
    space.planes.append(plane)

    plane = Plane()
    plane.a = -5.52696244327e-09
    plane.b = 0.025390625
    plane.c = 0.00416564941406
    plane.d = 0.0124969482422
    space.planes.append(plane)

    plane = Plane()
    plane.a = 0.0253906045109
    plane.b = 0.0
    plane.c = 0.0155716277659
    plane.d = 0.0467148832977
    space.planes.append(plane)

#    print "PUBLISHING: %s" % space
    publisher.publish(space)

    # ----

    space = Convex_Space()
    space.sensor      = "/overhead_cam"
    space.algorithm   = "ian3"
    space.aux_payload = "ian3:red"

    space.planes = []

    plane = Plane()
    plane.a = 3.46700481657e-08
    plane.b = -0.048828125
    plane.c = -0.0261306762695
    plane.d = -0.0783920288086
    space.planes.append(plane)

    plane = Plane()
    plane.a = -0.0507812052965
    plane.b = 0.0
    plane.c = -0.0324326232076
    plane.d = -0.0972978696227
    space.planes.append(plane)

    plane = Plane()
    plane.a = -3.13801891139e-08
    plane.b = 0.048828125
    plane.c = 0.0236511230469
    plane.d = 0.0709533691406
    space.planes.append(plane)

    plane = Plane()
    plane.a = 0.0507812090218
    plane.b = 0.0
    plane.c = 0.0299530699849
    plane.d = 0.0898592099547
    space.planes.append(plane)

#    print "PUBLISHING: %s" % space
    publisher.publish(space)


    # END OF RECORDED PLANES

    return

    








    # this is a horizontal slicer
    space = Convex_Space()

    plane = Plane()
    plane.a = 0
    plane.b = 0
    plane.c = 1
    plane.d = -1

    space.planes.append(plane);

    plane = Plane()
    plane.a =  0
    plane.b =  0
    plane.c = -1
    plane.d =  0

    space.planes.append(plane);

    space.sensor      = "horizontal_slicer"
    space.algorithm   = "horizontal_slicer"
    space.aux_payload = "slicer:horizontal"

    publisher.publish(space)

    print "Just published a horizontal_slicer"
    rospy.Rate(1).sleep()

    

    # this generates something approximating the sort of pyramid which
    # camera2boundingBox would produce.
    #
    # Characteristic points:
    #     Camera:
    #         (0, 0, 3)
    #     Planes:
    #         (3, 3, 0)
    #         (3, 6, 0)
    #         (6, 3, 0)
    #         (6, 6, 0)

    space = Convex_Space()

    plane = Plane()
    plane.a = -1
    plane.b =  0
    plane.c = -1
    plane.d =  3

    space.planes.append(plane)
    
    plane = Plane()
    plane.a =  0
    plane.b = -1
    plane.c = -1
    plane.d =  3

    space.planes.append(plane)
    
    plane = Plane()
    plane.a =  0.5
    plane.b =  0
    plane.c =  1
    plane.d = -3

    space.planes.append(plane)
    
    plane = Plane()
    plane.a =  0
    plane.b =  0.5
    plane.c =  1
    plane.d = -3

    space.planes.append(plane)

    space.sensor      = "pseudo_pyramid"
    space.algorithm   = "pseudo_pyramid"
    space.aux_payload = ""

    publisher.publish(space)

    print "Just published a pseudo-pyramid"
    rospy.Rate(1).sleep()





    # test a few passes where we gradually reduce the box size.
    for radius in inclusive_range(10*1000, 5*1000, -1000):
        space = Convex_Space()

        plane = Plane()
        plane.a = 1
        plane.b = 0
        plane.c = 0
        plane.d = -radius
        print "plane: %s" % plane
        space.planes.append(plane)

        plane = Plane()
        plane.a = -1
        plane.b = 0
        plane.c = 0
        plane.d = -radius
        space.planes.append(plane)

        plane = Plane()
        plane.a = 0
        plane.b = 1
        plane.c = 0
        plane.d = -radius
        space.planes.append(plane)

        plane = Plane()
        plane.a = 0
        plane.b = -1
        plane.c = 0
        plane.d = -radius
        space.planes.append(plane)

        plane = Plane()
        plane.a = 0
        plane.b = 0
        plane.c = 1
        plane.d = -radius
        space.planes.append(plane)

        plane = Plane()
        plane.a = 0
        plane.b = 0
        plane.c = -1
        plane.d = -radius
        space.planes.append(plane)

        space.sensor      = "plane_slicer"
        space.algorithm   = "plane_slicer"
        space.aux_payload = "plane_slicer_radius=%d" % radius

        publisher.publish(space)

        print "Just published a plane-slicing space with radius=%d" % radius
        rospy.Rate(1).sleep()



    # remember that the initial box, defined in 3d_intersector, is +- 10*1000 in each dimension.
    #
    # With radius == 10*1000, we get 3 key points, which are the exact centers of 3 different
    # faces; the plane slices through those.  However, it works out that the plane also slices
    # through the 3 nearby corners of the original box.
    #
    # With radius == 20*1000, we get 3 key points, but these are now midpoints of edges, instead
    # of the center of faces.  Thus, each plane just slices off one corner of the cube.
    #
    # We'll test with 20k, then 10k, and see how our intersector handles it.
    for radius in inclusive_range(20*1000, 10*1000, -10*1000):
        space = Convex_Space()

        for a in range(-1, 1+1, 2):
            for b in range(-1, 1+1, 2):
                for c in range(-1, 1+1, 2):
                    plane = Plane()
                    plane.a = a
                    plane.b = b
                    plane.c = c
                    plane.d = -radius  # need negative d value, since we are adding it to the left-hand side of the inequality, and then checking if the value is small enough.

                    space.planes.append(plane)

        space.sensor     = "corner_slicer"
        space.algorithm  = "corner_slicer:radius=%d" % radius
        space.aux_payload = "corner_slicer_radius=%d" % radius

        publisher.publish(space)

        print "Just published a corner-slicing space with radius=%d" % radius
        rospy.Rate(1).sleep()



if __name__ == '__main__':
    try:
        init()
        rospy.spin()
    except rospy.ROSInterruptException: pass

